/*
 * servo.cpp
 *
 *  Created on: 2024年3月9日
 *      Author: 陈朝
 */
#include "servo.h"


// 全局变量
std::mutex data_mutex;

// 计算舵机占空比
static uint16_t calculate_duty(float angle, const pwm_info_t *pwm_cfg) {
    return (uint16_t)((float)pwm_cfg->duty_max / (1000.0 / (float)pwm_cfg->freq) * (0.5 + (180-angle) / 90.0));
}

void servo_init(ServoController *servo) {
    // 获取PWM设备信息
    pwm_get_dev_info(SERVO_PWM_DEVICE, &servo->pwm_cfg);
    
    // 初始化舵机到中位
    servo->current_angle = SERVO_CENTER_ANGLE;
    servo->last_error = 0.0f;
    pwm_set_duty(SERVO_PWM_DEVICE, calculate_duty(servo->current_angle, &servo->pwm_cfg));
}

void servo_set_angle(ServoController *servo, float angle) {
    // 限制角度范围
    if (angle < SERVO_LEFT_MAX) angle = SERVO_LEFT_MAX;
    if (angle > SERVO_RIGHT_MAX) angle = SERVO_RIGHT_MAX;
    
    // 平滑过渡
    float smoothed_angle = servo->current_angle * (1.0f - SMOOTH_FACTOR) + angle * SMOOTH_FACTOR;
    
    // 更新角度并设置PWM
    servo->current_angle = smoothed_angle;
    pwm_set_duty(SERVO_PWM_DEVICE, calculate_duty(smoothed_angle, &servo->pwm_cfg));
}
//加入前瞻控制，使得舵机转向更丝滑
void servo_control_by_error(ServoController *servo, int error, int max_error) {
    // 计算归一化误差 (-1.0 ~ 1.0)
    float normalized_error = (float)error / max_error;
    
    // 计算误差变化率 (微分项)
    float error_diff = normalized_error - servo->last_error;
    servo->last_error = normalized_error;
    
    // 动态调整比例系数 - 根据误差变化率调整
    float dynamic_kp = KP_SMALL;
    if(fabs(normalized_error) > ERROR_THRESHOLD) {
        dynamic_kp = KP_SMALL + (KP_LARGE-KP_SMALL) * 
                    (fabs(normalized_error)-ERROR_THRESHOLD)/(1.0f-ERROR_THRESHOLD);
    }
    
    // 前瞻控制 - 使用图像上半部分的误差预测
    float predict_error = 0;
    int predict_count = 0;
    for(int i = 0; i < UVC_HEIGHT/2; i++) {
        if(midline[i] > 0) {
            predict_error += midline[i] - (UVC_WIDTH / 2);
            predict_count++;
        }
    }
    if(predict_count > 0) {
        predict_error = predict_error / predict_count / max_error;
    }
    
    // 计算角度偏移 (比例 + 微分 + 前瞻)
    float angle_offset = normalized_error * (SERVO_RIGHT_MAX - SERVO_CENTER_ANGLE) * dynamic_kp
                       + error_diff * (SERVO_RIGHT_MAX - SERVO_CENTER_ANGLE) * KP_DIFF
                       + predict_error * (SERVO_RIGHT_MAX - SERVO_CENTER_ANGLE) * 0.2f; // 前瞻系数
    
    // 计算目标角度
    float target_angle = SERVO_CENTER_ANGLE + angle_offset;
    
    // 设置舵机角度
    servo_set_angle(servo, target_angle);
}


void servo_use() {
    ServoController my_servo;
    servo_init(&my_servo);

    while(1) {
        int error;
        {
            //std::lock_guard<std::mutex> lock(data_mutex);
            // 使用图像下半部分的中线平均值作为误差
            int sum = 0;
            int count = 0;
            for(int i = UVC_HEIGHT/2; i < UVC_HEIGHT; i++) {
                if(midline[i] > 0) {
                    sum += midline[i] - (UVC_WIDTH / 2);
                    count++;
                }
            }
            error = count > 0 ? sum / count : 0;
        }
        
        servo_control_by_error(&my_servo, error, UVC_WIDTH / 2);
        //std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 10ms控制周期
    }
}
